#!/usr/bin/env python

import time
import math
import os
import sys


path = os.path.abspath(".")
# 核心
sys.path.insert(0,path + "src/function_pkg/scripts")

from robotic_arm_pkg.robotic_arm import *



def m_callback(data):
    print("MCallbackMCallbackMCallback")
    # 判断接口类型
    if data.codeKey == MOVEJ_CANFD_CB:
        # 角度透传
        print("透传结果:", data.errCode)
        print("当前角度:", data.joint[0], data.joint[1], data.joint[2], data.joint[3], data.joint[4],
              data.joint[5])
    elif data.codeKey == MOVEP_CANFD_CB:  # 位姿透传
        print("透传结果:", data.errCode)
        print("当前角度:", data.joint[0], data.joint[1], data.joint[2], data.joint[3], data.joint[4],
              data.joint[5])
        print("当前位姿:", data.pose.position.x, data.pose.position.y, data.pose.position.z,
              data.pose.euler.rx, data.pose.euler.ry, data.pose.euler.rz)
    elif data.codeKey == FORCE_POSITION_MOVE_CB:  # 力位混合透传
        print("透传结果:", data.errCode)
        print("当前力度：", data.nforce)
    else:
        print("neither")


def test01():
    callback = CANFD_Callback(m_callback)
    robot = Arm(RM65, "192.168.1.18", callback)
    try:
        acc_speed_ratio = 20
        joint_dir = -1
        count = 1
        while acc_speed_ratio < 100:
            print(f"当前角度: { robot.Get_Joint_Degree() }")
            robot.Joint_Step_Cmd(6, joint_dir * 300, acc_speed_ratio)
            acc_speed_ratio += 5
            count += 1
            joint_dir = (-1) ** count
            print(f"joint_dir: { joint_dir }, acc_speed_ratio: { acc_speed_ratio }")
    except Exception as e:
        print(f"error: { e }")
    robot.RM_API_UnInit()
    robot.Arm_Socket_Close()
    return


def test02():
    callback = CANFD_Callback(m_callback)
    robot = Arm(RM65, "192.168.1.18", callback)
    joint1 = [0, -30, -30, 0, -40, 300]
    joint2 = [-30, -10, -10, -10, -10, 300]
    zero = [0, 0, 0, 0, 0, 0]
    time.sleep(3)
    try:
        robot.Movej_Cmd(zero, 20, 0)
        print(f"当前角度: {robot.Get_Joint_Degree()}")
        robot.Movej_Cmd(joint1, 20, 0)
        print(f"当前角度: {robot.Get_Joint_Degree()}")
        robot.Movej_Cmd(zero, 20, 0)
        print(f"当前角度: {robot.Get_Joint_Degree()}")
        robot.Movej_Cmd(joint2, 20, 0)
        res, joint, pose, arm_err, sys_err = robot.Get_Current_Arm_State()
        for i in range(50):
            pose[0] += 0.001
            robot.Movep_CANFD(pose, False)
            time.sleep(0.02)
        robot.Movej_Cmd(zero, 20, 0)

    except Exception as e:
        print(f"error: { e }")
    robot.RM_API_UnInit()
    robot.Arm_Socket_Close()


def test03():
    callback = CANFD_Callback(m_callback)
    robot = Arm(RM65, "192.168.1.18", callback)
    retval, frame = robot.Get_Current_Work_Frame()
    if retval == 0:
        print('当前工作坐标系：', frame.frame_name.name)
        print('当前工作坐标系位置：', frame.pose.position.x, frame.pose.position.y, frame.pose.position.z)
    else:
        print(f'获取当前坐标系失败:{retval}')
        # 获取指定坐标系
    retval, frame = robot.Get_Given_Work_Frame('new2')
    if retval == 0:
        print('指定工作坐标系位姿：', frame)
    else:
        print(f'获取指定坐标系失败:{retval}')
    # max_pos = robot.Get_Joint_Max_Pos()[1]
    # min_pos = robot.Get_Joint_Min_Pos()[1]
    # print(f"关节最大限位: { max_pos }\n"
    #       f"关节最小限位: { min_pos }")
    # for i in range(1, len(max_pos)+1):
    #     print(f"关节{i}最大限位弧度为: {max_pos[i-1]/180*Pi}")
    #     print(f"关节{i}最小限位弧度为: {min_pos[i - 1] / 180 * Pi}")
    # robot.Movel_Cmd()
    robot.RM_API_UnInit()
    robot.Arm_Socket_Close()


def test04():
    callback = CANFD_Callback(m_callback)
    robot = Arm(RM65, "192.168.1.18", callback)
    zero = [0, 0, 0, 0, 0, 0]
    print("三秒后开始演示")
    time.sleep(3)
    pose01 = [90, 0, 0, 0, 0, 0]
    pose02 = [90, 20, 20, 0, 10, 0]
    pose03 = [90, -20, -20, 0, -20, 0]
    robot.Movej_Cmd(zero, 20, 0)
    robot.Movej_Cmd(pose01, 20, 0)
    robot.Movej_Cmd(pose02, 20, 0)
    robot.Movej_Cmd(pose03, 20, 0)
    robot.Movej_Cmd(pose01, 20, 0)
    robot.Movej_Cmd(zero, 20, 0)
    # pose02 = [0, 20, 20, 0, 10, 0]
    pose04 = [0, -10, -40, 0, -50, 0]
    robot.Movej_Cmd(zero, 20, 0)
    robot.Movej_Cmd(pose04, 20, 0)
    robot.Movej_Cmd(zero, 20, 0)
    robot.Movej_Cmd(pose04, 20, 0)
    robot.Movej_Cmd(zero, 20, 0)
    demo1(robot)
    robot.Movej_Cmd(zero, 20, 0)
    robot.RM_API_UnInit()
    robot.Arm_Socket_Close()


def demo1(robot):
    ret = robot.Movej_Cmd([18.44, -10.677, -124.158, -15, -71.455], 30, 0)
    if ret != 0:
        print("设置初始位置失败:" + str(ret))

    for num in range(0, 3):
        po1 = [0.186350, 0.062099, 0.2, 3.141, 0, 1.569]

        ret = robot.Movel_Cmd(po1, 30, 0)
        if ret != 0:
            print("Movel_Cmd 1 失败:" + str(ret))

        po2 = [0.21674, 0.0925, 0.2, 3.141, 0, 1.569]
        po3 = [0.20785, 0.114, 0.2, 3.141, 0, 1.569]

        ret = robot.Movec_Cmd(po2, po3, 30, 0, 0)
        if ret != 0:
            print("Movec_Cmd 1 失败:" + str(ret))

        po4 = [0.164850, 0.157, 0.2, 3.141, 0, 1.569]

        ret = robot.Movel_Cmd(po4, 30, 0)
        if ret != 0:
            print("Movel_Cmd 2 失败:" + str(ret))

        po5 = [0.186350, 0.208889, 0.2, 3.141, 0, 1.569]
        po6 = [0.20785, 0.157, 0.2, 3.141, 0, 1.569]

        ret = robot.Movec_Cmd(po5, po6, 30, 0, 0)
        if ret != 0:
            print("Movel_Cmd 2 失败:" + str(ret))

    print("cycle draw 8 demo success")


def test05():
    callback = CANFD_Callback(m_callback)
    robot = Arm(RM65, "192.168.1.18", callback)
    joint_info = robot.Get_Joint_Degree()
    print(joint_info)
    robot.RM_API_UnInit()
    robot.Arm_Socket_Close()


def test06():
    callback = CANFD_Callback(m_callback)
    robot = Arm(RM65, "192.168.1.18", callback)
    zero = [0, 0, 0, 0, 0, 0]
    pose01 = [50.65700149536133, -12.105999946594238, 130.02099609375, 5.932000160217285, 51.553001403808594, 7.793000221252441]
    pose02 = [52.145999908447266, 15.027999877929688, 132.5399932861328, 7.216000080108643, 28.500999450683594, 7.785999774932861]
    pose03 = [41.13399887084961, 48.854000091552734, 34.108001708984375, 4.375, 80.56400299072266, 12.760000228881836]
    robot.Movej_Cmd(zero, 20, 0)
    robot.Movej_Cmd(pose01, 20, 0)
    robot.Movej_Cmd(pose02, 20, 0)
    time.sleep(2)
    robot.Movej_Cmd(pose02, 20, 0)
    robot.Movej_Cmd(pose03, 20, 0)
    time.sleep(1)
    robot.Movej_Cmd(zero, 20, 0)
    robot.RM_API_UnInit()
    robot.Arm_Socket_Close()
    
    pass


if __name__ == "__main__":
    # print(os.path.abspath("."))
    test06()


# def templates():
    # callback = CANFD_Callback(m_callback)
    # robot = Arm(RM65, "192.168.1.18", callback)
    # robot.RM_API_UnInit()
    # robot.Arm_Socket_Close()
    # pass
